#include "controller.h"
#include <iostream>
#include <math.h>

Controller::Controller()
{
	for(int i = 0; i < 4; i++)
		resp[i] = 0;
}

char* Controller::init(const Message* msg)
{
	std::cerr<<"Initializing\n";
	maxX = msg->mapX / 2.00;
	minX = -maxX;
	maxY = msg->mapY / 2.00;
	minY = -maxY;
	maxTime = msg->mapTimeLimit;
	minSensor = msg->minSensor;
	maxSensor = msg->maxSensor;
	maxLinearSpeed = msg->maxLinearSpeed;
	maxTurnSpeed = msg->maxTurnSpeed;
	maxHardTurnSpeed = msg->maxHardTurnSpeed;
	home = new Home(0.0, 0.0, 5.0);

	speed = 0;
	speedState = '-';
	turnState = '-';
	exp_speed = 0;
	exp_speedState = '-';
	exp_turnState = '-';
		
	resp[0] = 0;
	return resp;
}

char* Controller::notify(const Message* msg)
{
	switch(msg->mType)
	{
		case 'B':
			std::cerr<<"Hit the boulder at " << msg->timestamp << " msec\n";
		break;
		case 'C':
			std::cerr<<"Fall into the crater at " << msg->timestamp << " msec\n";
		break;
		case 'K':
			std::cerr<<"Killed by a martian at " << msg->timestamp << " msec\n";
		break;
		case 'S':
			std::cerr<<"Home, sweet home at " << msg->timestamp << " msec\n";
		break;
	}
	resp[0] = 0;
	return resp;
}

char* Controller::makeDecision(const Message* msg)
{
	//TODO
	timestamp = msg->timestamp;
	unsigned long timelimit = maxTime - timestamp;
	bool iSeeHome = false; //when this is true, logic will be simpler
	

	//first of all, put new objects on the map
	while(martians.size())
	{
		MapObject * obj = martians.front();
		delete obj;
		martians.pop_front();
	}
	for (std::list<MapObject*>::iterator iter = msg->objList->begin(); iter != msg->objList->end(); iter++)
	{
		char objType = (*iter)->getType();
		switch(objType)
		{
			case 'h':
				iSeeHome = true;
				//std::cerr<<"Weeha! I see home!\n"; //nothing to do more, home is static & already on the map
			break;
			case 'm':
				martians.push_back((Martian*)*iter);
			break;
			case 'b':
			case 'c':
				bool _copy = false;
				StaticObject* obj = (StaticObject*)*iter;
				for (std::list<StaticObject*>::iterator iter2 = map.begin(); iter2 != map.end(); iter2++)
				{
					if((*iter2)->getType() != objType)
						continue;
					if(((*iter2)->getX() != obj->getX()) || 
					   ((*iter2)->getY() != obj->getY()) ||
					   ((*iter2)->getRadius() != obj->getRadius()))
						continue;
					_copy = true;
					break;
				}
				if (!_copy)
					map.push_back(obj);
				else
					delete obj;
			break;
		}
	}
	//end put new objects
	
	if(iSeeHome)
	{
		double homeAngle = getAngle(Point(msg->x, msg->y), Point(home->getX(), home->getY()));
		if(distanceLinePoint(Position(msg->x, msg->y, msg->turn), Point(home->getX(), home->getY())) < home->getRadius())
				{
					std::cerr<<"Test:"<<distanceLinePoint(Position(msg->x, msg->y, msg->turn), Point(home->getX(), home->getY()))<<"\n";
					homeAngle = msg->turn;
				}
		double homeDist = distancePoints(Point(msg->x, msg->y), Point(home->getX(), home->getY()));
		std::cerr<<"Home is at "<<homeDist<<"@"<<homeAngle<<"\n";
		std::cerr<<"I am at ["<<msg->x<<","<<msg->y<<"]@"<<msg->turn<<", sp="<<msg->speed<<"\n";
		StaticObject* obstacle = isWayClear(Position(msg->x, msg->y, homeAngle), homeDist);
		if(obstacle == 0)
		{
			//path is clear
			//std::cerr<<"GO GO GO\n";
			//Do we need to care about old point?
			//if((sweetPoint._x == 0.0) && (sweetPoint._y == 0.0))
			//{
				
				sweetPosition.coord._x = msg->x;
				sweetPosition.coord._y = msg->y;
				sweetPosition.angle = homeAngle;
				goSweetPosition(msg);
			//}
			//else
			//{
			//	double oldHomeDist = distancePoints(sweetPoint, Point(home->getX(), home->getY()));
			//	if(oldHomeDist < homeDist)
			//	{
			//		
			//	}
			//}
		}
		else
		{
			std::cerr << "Obstacle found, recomputing\n";
			std::cerr << "Coords are: [" << obstacle->getX() << "," <<obstacle->getY() << "] r = " << obstacle->getRadius() << "\n";
			std::cerr << "My coords are: [" << msg->x << "," << msg->y << "]@" << msg->turn << "\n";
			//TODO find some point near obstacle,
			//such that isWayClear() to that point returns 0 
			// and isWayClear() from that point to next point returns 0 or other obstacle
			
			// we may be need a waypoint list
		}
	}
	else if(!((sweetPosition.coord._x == 0.0) && (sweetPosition.coord._y == 0.0)))// was "good" position
	{
		goSweetPosition(msg);
	}
	else //try to find way home
	{
		//TODO	A1 generic find path strategy
		std::cerr<<"Help me, I am lost!\n";
	}
	
	speed = msg->speed;
	last.coord._x = msg->x;
	last.coord._y = msg->y;
	last.angle = msg->turn;
	speedState = msg->vehicleSpeedState;
	turnState = msg->vehicleTurnState;
	//TODO expected?
	return resp;
}

char* Controller::cleanup(const Message* msg)
{
	std::cerr << "End of round, time is " << msg->timestamp << ", score is " << msg->score << "\n";
	
	last.coord._x = last.coord._y = last.angle = speed = 0;
	speedState = '-';
	turnState = '-';
	expected.coord._x = expected.coord._y = expected.angle = exp_speed = 0;
	exp_speedState = '-';
	exp_turnState = '-';

	sweetPosition.coord._x = sweetPosition.coord._y = 0.0;

	while(martians.size())
	{
		MapObject * obj = martians.front();
		delete obj;
		martians.pop_front();
	}

	resp[0] = 0;
	return resp;
}

Controller::~Controller()
{
	while(map.size())
	{
		MapObject *obj = map.front();
		delete obj;
		map.pop_front();
	}
	while(martians.size())
	{
		MapObject * obj = martians.front();
		delete obj;
		martians.pop_front();
	}
	delete home;
}


/**
*	returns distance between two points
*/
double Controller::distancePoints(Point a, Point b)
{
	return sqrt((b._x - a._x) * (b._x - a._x) + (b._y - a._y) * (b._y - a._y));
}

/**
*	returns distance between line and point
*/
double Controller::distanceLinePoint(Position p, Point b)
{
	double rad = p.angle / 180.0 * M_PI;
	//std::cerr<<"rad of "<<p.angle<<" is "<<rad<<"\n";
	//std::cerr<<"["<<p.coord._x<<","<<p.coord._y<<"] - ["<<b._x<<","<<b._y<<"]\n";
	return fabs((b._x - p.coord._x) * sin(rad) - (b._y - p.coord._y) * cos(rad));
}

/**
*	returns point of crash of two moving objects or (0,0) if they are moving different directions
*	DOES NOT check for map limits, so point can be out of it
*	expected to be used for controller and martian
*/
Point Controller::crashPoint(Position a, Position b)
{
	Point res;
	double alpha = a.angle / 180.0 * M_PI;
	double betta = b.angle / 180.0 * M_PI;
	res._x = (b.coord._x * sin(betta) * cos(alpha) - a.coord._x * sin(alpha) * cos(betta) - 
			 (b.coord._y - a.coord._y) * cos(alpha) * cos(betta)) / sin(betta - alpha);
	res._y = (a.coord._y * cos(alpha) * sin(betta) - b.coord._y * sin(alpha) * cos(betta) +
			 (b.coord._x - a.coord._x) * sin(alpha) * sin(betta)) / sin(betta - alpha);
	if(!isPointAhead(a, res) || !isPointAhead(b, res))
	{
		res._x = res._y = 0.0;
	}
	return res;
}

/**
*	returns angle in degrees  -180 < angle <= 180
*/
double Controller::getAngle(Point a, Point b)
{
	double dist = distancePoints(a, b);
	double _sin = (b._y - a._y) / dist;
	double _cos = (b._x - a._x) / dist;
	double rad1 = acos(_cos);
	double rad2 = asin(_sin);
	double res = 0;
	if(fabs(rad1 - rad2) < delta)
		res = rad1 * 180.0 / M_PI;
	if(_sin > 0)
		res = (M_PI - rad2) * 180.0 / M_PI;
	else
		if(_cos > 0)
			res = rad2 * 180.0 / M_PI;
		else
			res = (2 * M_PI - rad2) * 180.0 / M_PI;
	if(res <= -180.0)
		return res + 360.0;
	else
		if(res > 180.0)
			return res - 360.0;
		else
			return res;
}

void Controller::calculate()
{
	//seems like junk
}

/**
*	returns point where will be crash with STATIC OBJECT or (0,0) if everything is OK and we can move directly 
*/
StaticObject* Controller::isWayClear(Position p, double dist)
{
	for(std::list<StaticObject*>::iterator iter = map.begin(); iter != map.end(); iter++)
	{
		double minDist = .50 + (*iter)->getRadius();
		Point test((*iter)->getX(), (*iter)->getY());
		double actualDist = distanceLinePoint(p, test);
		if((actualDist <= minDist) && (isPointAhead(p, test))) //we can use it here, because distance > radius
		{
			double a = p.angle;
			if (a < 90.0)
				a+= 90.0;
			else
				a-= 270.0;
			Point crashPt = crashPoint(p, Position(test, a));
			if((crashPt._x == 0) && (crashPt._y == 0))
			{
				if (a < 0.0)
					a+= 180.0;
				else
					a-= 180.0;
				crashPt = crashPoint(p, Position(test, a));
			}
			if((crashPt._x == 0) && (crashPt._y == 0))
			{
				std::cerr<<"Bad day, errors in crashPt func :(\n";
			}
			else
			{
				//we get some point inside obstacle
				double crashDist = distancePoints(p.coord, crashPt);
				if(crashDist < dist) //and only there we can say that we crash in that object
					return *iter;
			}
		}
	}
	return 0;
}

/**
*	expects that distance between line and point is 0
*	returns whether the point will be reached if continue to move that direction
*/
bool Controller::isPointAhead(Position p, Point pt)
{
	double dist = distancePoints(p.coord, pt);
	double angle = p.angle / 180.0 * M_PI;
	Point test(p.coord._x + dist * cos(angle) / 2.0 , p.coord._y + dist * sin(angle) / 2.0);
	return (dist > distancePoints(test, pt));
}

/**
*	main target of this function is try to get to "sweetPosition"
*/
void Controller::goSweetPosition(const Message* msg)
{
	Position curPos(msg->x, msg->y, msg->turn);
	double minD = maxTurnSpeed / 10.0;
	double maxD = maxHardTurnSpeed / 10.0;
	if((curPos.coord._x == sweetPosition.coord._x) &&
	   (curPos.coord._y == sweetPosition.coord._y))
	{
		if(fabs(curPos.angle - sweetPosition.angle) < minD)
		{
			//wow! just move ahead
			resp[0] = 'a';
			if(msg->vehicleTurnState == '-')
			{
				resp[1] = ';';
				resp[2] = 0;
			}
			if((msg->vehicleTurnState == 'l')||((msg->vehicleTurnState == 'L')))
			{
				resp[1] = 'r';
				resp[2] = ';';
			}
			if((msg->vehicleTurnState == 'r')||((msg->vehicleTurnState == 'R')))
			{
				resp[1] = 'l';
				resp[2] = ';';
			}
		}
		else
		{
			//just turn
			double deltaAngle = sweetPosition.angle - curPos.angle;
			bool turnLeft;
			if (deltaAngle > 0.0)
				turnLeft = true;
			else
				turnLeft = false; //turn right :)
			if(turnRightDirection(msg->vehicleTurnState, turnLeft))
			{
				if(deltaAngle < 2 * maxD) 
				{
					if(msg->vehicleTurnState == (turnLeft? 'L' : 'R'))
					{
						resp[0] = (turnLeft ? 'r' : 'l');
						resp[1] = ';';
						resp[2] = 0;
					}
				}
				else
				{
					resp[0] = (turnLeft ? 'l' : 'r');
					resp[1] = ';';
					resp[2] = 0;
				}
				
			}
			else
			{
				resp[0] = (turnLeft ? 'l' : 'r');
				resp[1] = ';';
				resp[2] = 0;
			}
		}
	}
	else
		resp[0] = 0;
	//TODO some algo needed
}

bool Controller::turnRightDirection(char ch, bool turnLeft)
{
	if(ch == '-')
		return false;
	if((ch == 'l') || (ch == 'L'))
		return (turnLeft ? true : false);
	if((ch == 'r') || (ch == 'R'))
		return (turnLeft ? false : true);
}
